/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This will be a huikea paikannus vekotin 
*/

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include "CScanMatchInterface.h"
#include "C3DCorrelationScanMatch.h"
#include "CAHistogramECorrelationScanMatch.h"
#include "CParticleFilter.h"
#include "CLineMapRawScanMCL.h"
#include "CCornerMapMCL.h"
#include "ownutils.h"

/// For reading the data from file
int readScanLineAvant180(FILE *f,ScanMatch::scan &meas){
		int time_s;
		int time_us;
		int i;
		char c;
		fscanf(f,"%d %d ",&time_s,&time_us);
	//fprintf(stderr,"1 ");
		for(i=0;i<180;i++){
				fscanf(f,"%f",&meas.a[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<180){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"2 ");
		for(i=0;i<180;i++){
				fscanf(f,"%f",&meas.r[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<180){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"3 \n");
		meas.N = 180;
		return 0;
}

void scan2absoluteXY( ScanMatch::scan &s, float *x, float *y, ScanMatch::pose &pose){
	int i;
	if(x==NULL || y==NULL) return;
	
	for(i=0;i<s.N;i++){
		x[i]=s.r[i]*cos(s.a[i]+pose.a)+pose.x;
		y[i]=s.r[i]*sin(s.a[i]+pose.a)+pose.y;
	}
}

float minMSD(float *x,float *y, float px,float py, int N, int &mini, float &dx2, float &dy2){
	float dist2=100000;
	float d2;
	int minInd = 0;
	float dx,dy;
	for(int i=0;i<N;i++){
		dx = (x[i]-px)*(x[i]-px);
		dy = (y[i]-py)*(y[i]-py);
		d2 = dx + dy;
		if(d2<dist2){ 
			dist2 = d2;
			dx2 = dx;
			dy2 = dy;
			minInd = i;
		}
	}
	mini = minInd;
	return dist2;
}

void computeMSE(ScanMatch::scan &s1,ScanMatch::scan &s2,ScanMatch::pose &p1,ScanMatch::pose &p2, 
								  float &x_err ,float &y_err){
	float *x1 = (float *) malloc(s1.N*sizeof(float));  
	float *y1 = (float *) malloc(s1.N*sizeof(float));
	float *x2 = (float *) malloc(s2.N*sizeof(float));  
	float *y2 = (float *) malloc(s2.N*sizeof(float));
	
	long int ss1,us1,ss2,us2,sec,us,ms;
	ownTime_GetTimeOfDay(&ss1,&us1);
	
	scan2absoluteXY(s1,x1,y1,p1);
	scan2absoluteXY(s2,x2,y2,p2);
	FILE *f = fopen("scans.txt","wt");
	for(int i=0;i<s1.N;i++){
			fprintf(f,"%.2f %.2f %.2f %.2f\n",x1[i],y1[i],x2[i],y2[i]);
				
	}
	fclose(f);
	
	int mini;
	float dist=0,d;
	float dx2=0,dy2=0,ddx,ddy;
	int num=0;
	for(int i=0;i<s1.N;i++){
		d = minMSD(x2,y2,x1[i],y1[i],s2.N,mini,ddx,ddy);
		if(d<1.0){
			dist += d; 
			dx2 += ddx;
			dy2 += ddy;
			num++;
		}
	}	
	
	ownTime_GetTimeOfDay(&ss2,&us2);
	sec = ss2-ss1;
	us = us2-us1;
	ms = sec*1000+us/1000;
	float fms=(float)sec*1000.0+(float)us/1000.0;
	//fprintf(stderr,"It took %.4f ms to calculate\n",fms);
	
	//fprintf(stderr,"MSE= %.2f MSE(X)=%.2f MSE(Y)=%.2f", dist/num,dx2/num,dy2/num);
	//fprintf(stderr," ME= %.2f ME(X)=%.2f ME(Y)=%.2f\n", sqrt(dist/num),sqrt(dx2/num),sqrt(dy2/num));
	//x_err = sqrt(dx2/num);
	//y_err = sqrt(dy2/num);
	if(num!=0){
  	x_err = sqrt(dx2/num);
  	y_err = sqrt(dy2/num);
	}
	else{
		x_err = -666;
		y_err = -666;
	}
	free(x1);
	free(x2);
	free(y1);
	free(y2);
}

void scan2absoluteXY( mcl::scan &s, float *x, float *y, mcl::pose &pose){
	int i;
	if(x==NULL || y==NULL) return;
	
	for(i=0;i<s.N;i++){
		x[i]=s.r[i]*cos(s.a[i]+pose.a)+pose.x;
		y[i]=s.r[i]*sin(s.a[i]+pose.a)+pose.y;
	}
}


void computeMSE(mcl::scan &s1,mcl::scan &s2,mcl::pose &p1,mcl::pose &p2, 
	float &x_err ,float &y_err){
		float *x1 = (float *) malloc(s1.N*sizeof(float));  
		float *y1 = (float *) malloc(s1.N*sizeof(float));
		float *x2 = (float *) malloc(s2.N*sizeof(float));  
		float *y2 = (float *) malloc(s2.N*sizeof(float));
		
		long int ss1,us1,ss2,us2,sec,us,ms;
		ownTime_GetTimeOfDay(&ss1,&us1);

		scan2absoluteXY(s1,x1,y1,p1);
		scan2absoluteXY(s2,x2,y2,p2);
	
		
		
		int mini;
		float dist=0,d;
		float dx2=0,dy2=0,ddx,ddy;
		int num=0;
		for(int i=0;i<s1.N;i++){
			d = minMSD(x2,y2,x1[i],y1[i],s2.N,mini,ddx,ddy);
			if(d<9.0){
				dist += d; 
				dx2 += ddx;
				dy2 += ddy;
				num++;
			}
		}	

		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss1;
		us = us2-us1;
		ms = sec*1000+us/1000;
		float fms=(float)sec*1000.0+(float)us/1000.0;
	//fprintf(stderr,"It took %.4f ms to calculate\n",fms);
	
	//fprintf(stderr,"MSE= %.2f MSE(X)=%.2f MSE(Y)=%.2f", dist/num,dx2/num,dy2/num);
	//fprintf(stderr," ME= %.2f ME(X)=%.2f ME(Y)=%.2f\n", sqrt(dist/num),sqrt(dx2/num),sqrt(dy2/num));
	//x_err = sqrt(dx2/num);
	//y_err = sqrt(dy2/num);
		if(num != 0){
		  x_err = sqrt(dx2/num);
		  y_err = sqrt(dy2/num);
		}
		else{
			x_err = -666;
			y_err = -666;
		}
	free(x1);
	free(x2);
	free(y1);
	free(y2);
}



int main(void){
		CScanMatchInterface *matcher;
		CScanMatchInterface *gmatcher;
		ScanMatch::pose pos,odo_cur,odo_ref,est,rpos;   ///< The estimated position
		ScanMatch::scan ref,smref;   ///< The Reference scan 
		ScanMatch::scan meas;  ///< The Reference scan 
		FILE *f,*out;
		CLineMapRawScanMCL mcl,mcl_global;
		CCornerExtractor extractor;
  
		ref.allocate(180);
		meas.allocate(180);
		smref.allocate(180);
		out = fopen("result.txt","wt");
		//f = fopen("avat180.txt","rt");initPose.set(-4,-4,M_PI/4);
		f = fopen("capture2_hitaastiympariareenaa.txt","rt"); //initPose.set(-4,-1,M_PI/4);
		//f = fopen("capture3_jarin_haroily1.txt","rt"); //initPose.set(-2,-3,M_PI/2);
		//f = fopen("capture4_jussin_haroilu1.txt","rt");//initPose.set(-1,-5,M_PI/4);
		
		if(f==NULL){
				fprintf(stderr,"Error opening data file\n");
				return 1;
		}
  	readScanLineAvant180(f,ref);
  	pos.set(0,0,0);
		for(int i=0;i<ref.N;i++){
				smref.r[i] = ref.r[i];
				smref.a[i] = ref.a[i];
		}
		smref.N=180;
		fprintf(stderr,"The reference scan has %d measurements\n",ref.N);
  ///Initializing new matcher
  //matcher = new C3DCorrelationScanMatch();
  /*  
		matcher = new C3DCorrelationScanMatch(4,
		4,
		6,
		0.05,
		0.05,
		(.5/57.29578),
		0.03,
		361);*/

  
		matcher = new CAHistogramECorrelationScanMatch(
						0.6,   // Search area in meters 
		        0.9);     // Search angle in radians
		gmatcher = new CAHistogramECorrelationScanMatch(
				                  1.0,   // Search area in meters 
		                      0.9);     // Search angle in radians
		
  ///< FIXME: tarkistukset interfaceen!! 
		matcher->setReferenceScan(ref, pos); 
		
		mcl::pose initPose,p1,p2; 
		mcl::pose initVariance;
		mcl::pose dP;
		mcl::pose final;
		mcl::scan measurement,vm;
		mcl::pose variance;
		//initPose.set(-1,-5,M_PI/4);
		
		//initPose.set(-2,-3,M_PI/2);
		initPose.set(-3,-1,M_PI/4);
		//initVariance.set(1.0,1.0,M_PI/8);
		
		measurement.allocate(180);
		vm.allocate(180);
		
		mcl.initialize("map.txt",
										1.0, 
										800, 
										initPose, 
										initVariance,
										15,
										0);
		
		dP.set(0.25,0.25,M_PI * 10.0/180.0);
		/*
		mcl_global.initializeGlobal("map.txt",
												 1.0, 
												 dP, 
												 10,
												 0);
		*/
		int counter=0;
		long int ss1,us1,ss2,us2,sec,us,ms;
		int initSince=0;
		int errorcnt=0;
		while(!feof(f)){
				
				mcl.filt.saveToFile(counter);
								
				readScanLineAvant180(f,meas);
				ownTime_GetTimeOfDay(&ss1,&us1);
				///calculate differential movement ie initial estimate
				pos.set(0,0,0);
				matcher->update(meas,pos);    ///update
				matcher->getCurrentPose(pos); ///< Get the estimate
				dP.x = pos.x;
				dP.y = pos.y;
				dP.a = pos.a;
				/// Calculate Variances for MCL
				rpos.set(0,0,0);
				//ScanMatch::pose esti;
				//esti.set(-pos.x,-pos.y,-pos.a);
				computeMSE(smref,meas,rpos,pos,variance.x,variance.y);
				if(variance.x<0 || variance.y<0){
					variance.x = 0.5;
					variance.y = 0.5;
				}
				//variance.x = 0.1;
				//variance.y = 0.1;
				fprintf(stderr,"VAR(%.2f,%.2f) ",variance.x,variance.y);
				
				for(int i=0;i<meas.N;i++){
 					smref.r[i] = meas.r[i];
 					smref.a[i] = meas.a[i];
 				}
				matcher->getCurrentPose(rpos);
				
				pos.set(0,0,0);
				matcher->setReferenceScan(meas,pos); /// set reference to current position
				matcher->setCurrentPose(pos);

				
				measurement.set(meas.r,meas.a);
				measurement.N = meas.N;
				//variance.x = 0.1;
				//variance.y = 0.1;
				variance.a = M_PI/90;
				
				if(counter == 0){
						variance.set(0,0,0);
						dP.set(0,0,0);
				}
				
				mcl.runMCL(dP,variance, measurement);
				final = mcl.filt.getDistributionMean();
				//fprintf(stderr,"(%.2f %.2f %.2f) ",final.x,final.y,final.a);
				///Compute error
				
				mcl.getScan(vm, final);
				p1.set(0,0,0); p2.set(0,0,0);
				
 				computeMSE(vm,measurement,p1,p2,variance.x,variance.y);
				
				if(variance.x<0 || variance.y<0){
					variance.x = 0.5;
					variance.y = 0.5;
				}
 				if((variance.x > 0.75 ||variance.y > 0.75) && initSince>30 ){
 			  	if(errorcnt>4){
 					 fprintf(stderr,"Init:: ");
 					 initVariance.set(variance.x/2,variance.y/2,M_PI/4);
 					 mcl.reinitializeNR(final,initVariance);
 					
 					 final = mcl.filt.getDistributionMean();
 					 initSince = 0;
 					 errorcnt = 0;
 					}else{errorcnt++;}
 				}
 				initSince++;
			/////////////////////////////////////
// 				for(int i=0;i<vm.N;i++){
// 				  ref.r[i] = vm.r[i];
// 					ref.a[i] = vm.a[i];
// 				}
				for(int i=0;i<vm.N;i++){
					ref.r[i] = vm.r[i];
					ref.a[i] = vm.a[i];
				}
				pos.set(0,0,0);
				gmatcher->setReferenceScan(ref,pos); /// set reference to current position
				gmatcher->update(meas,pos);    ///update
				gmatcher->getCurrentPose(pos); ///< Get the estimate
				
				
				/////////////////////////////////////
				ownTime_GetTimeOfDay(&ss2,&us2);
				sec = ss2-ss1;
				us = us2-us1;
				ms = sec*1000+us/1000;
				float fms=(float)sec*1000.0+(float)us/1000.0;
				fprintf(stderr,"%d %.4f(ms) Err=(%.3f,%.3f) cor:(%.2f,%.2f,%.2f) final:(%.2f,%.2f,%.2f), dP:(%.2f,%.2f,%.2f)  N:(%d,%d,%d)\n",counter, fms,
								variance.x,variance.y,
								pos.x,pos.y,pos.a,
								final.x,final.y,final.a,
								dP.x,dP.y,dP.a,
								meas.N,measurement.N,vm.N
							 );
				//fprintf(stderr,"(%.2f %.2f %.2f) \n",final.x,final.y,final.a);
				//final = mcl_global.filt.getDistributionMean();
				
				fprintf(out,"%.2f %.2f %.2f ",final.x+pos.x,final.y+pos.y,final.a+pos.a);
				for(int scans=0;scans<meas.N;scans++) fprintf(out,"%.3f ",meas.r[scans]);
				for(int scans=0;scans<meas.N;scans++) fprintf(out,"%.3f ",meas.a[scans]);
				//for(int scans=0;scans<meas.N;scans++) fprintf(out,"%.3f ",vm.r[scans]);
				//for(int scans=0;scans<meas.N;scans++) fprintf(out,"%.3f ",vm.a[scans]);
				fprintf(out,"\n");
				fflush(out);
				counter++;
		}
		fclose(out);
  
  /// Deleting
		delete matcher;
  
		return 0;
}
